#ifndef __ZCM_DATA_H__
#define __ZCM_DATA_H__

#include <zcm/zcm-cpp.hpp>

#include <string>
#include <iostream>
#include "config.hpp"
#include "state_estimator_zcmt.hpp"
#include "rc_command_zcmt.hpp"
#include "upper_joint_control_zcmt.hpp"
#include "upper_joint_state_zcmt.hpp"
#include "system_init_zcmt.hpp"
#include "chassis_control_zcmt.hpp"
#include "chassis_state_zcmt.hpp"
#include "waist_control_zcmt.hpp"
#include "waist_state_zcmt.hpp"
#include "gripper_control_zcmt.hpp"
#include "gripper_state_zcmt.hpp"
#include "head_control_zcmt.hpp"
#include "head_state_zcmt.hpp"
#include "xbox_map_zcmt.hpp"
#include "robot_info_zcmt.hpp"
#include "error_log_zcmt.hpp"

class ZCM_Data
{
public:
    // 共享内存模式构造函数
    ZCM_Data();
    // UDP多播模式构造函数
    ZCM_Data(const std::string& udp_url);
    // 通用构造函数
    ZCM_Data(const std::string& internal_url, const std::string& udp_url);


    template <typename T>
    void publish(const std::string& channel, const T& msg) {
        _udpZCM.publish(channel, &msg);
        std::cout << "Published " << channel << std::endl;
    }


    ~ZCM_Data();
    
    // 枚举类

    // ZCM 实例
    zcm::ZCM _internalZCM;
    zcm::ZCM _udpZCM;

    // 数据成员
    // rc_command_zcmt rc_command{};   
    // state_estimator_zcmt body_state_simple{};

    system_init_zcmt system_init_cmd{};                     // 系统初始化控制指令
    system_init_zcmt system_init_state{};                   // 系统初始化反馈指令

    chassis_control_zcmt chassis_control{};                 // 底盘控制
    chassis_state_zcmt chassis_state{};                     // 底盘状态

    waist_control_zcmt waist_control{};                     // 腰部控制
    waist_state_zcmt waist_state{};                         // 腰部反馈

    head_control_zcmt head_control{};                       // 头部控制
    head_state_zcmt head_state{};                           // 头部状态

    upper_joint_control_zcmt upper_joint_control{};         // 上肢关节控制
    upper_joint_state_zcmt upper_joint_state{};             // 上肢关节状态

    gripper_control_zcmt gripper_control{};                 // 夹爪控制
    gripper_state_zcmt gripper_state{};                     // 夹爪状态

    xbox_map_zcmt xbox_map_control{};                           // Xbox手柄控制
    xbox_map_zcmt xbox_map_state{};                             // Xbox手柄状态

    state_estimator_zcmt imu_state{};                     // IMU状态

    robot_info_zcmt robot_info{};                           // 机器人基本信息
    error_log_zcmt error_log{};                             // 错误日志
    // 机器人状态
    bool is_info_received; // 系统是否初始化

    friend class H1Robot; // 添加这一行
    // todo   添加错误码和心跳的状态更新

private:
    // 初始化函数
    void initialize();
    void subscribeChannels();
    
    // ZCM 回调函数
    // ===== 状态反馈 =====
    void System_init_state_ZCM(const zcm::ReceiveBuffer* rbuf, 
                            const std::string& chan, 
                            const system_init_zcmt* msg);
    void Chassis_state_ZCM(const zcm::ReceiveBuffer* rbuf, 
                            const std::string& chan, 
                            const chassis_state_zcmt* msg);
    void Waist_state_ZCM(const zcm::ReceiveBuffer* rbuf, 
                          const std::string& chan, 
                          const waist_state_zcmt* msg);
    void Head_state_ZCM(const zcm::ReceiveBuffer* rbuf, 
                         const std::string& chan, 
                         const head_state_zcmt* msg);
    void upper_joint_state_ZCM(const zcm::ReceiveBuffer* rbuf, 
                            const std::string& chan, 
                            const upper_joint_state_zcmt* msg);
    void Gripper_state_ZCM(const zcm::ReceiveBuffer* rbuf, 
                        const std::string& chan, 
                        const gripper_state_zcmt* msg);
    // void RC_command_state_ZCM(const zcm::ReceiveBuffer* rbuf, 
    //                         const std::string& chan, 
    //                         const rc_command_zcmt* msg);
    void xbox_map_state_ZCM(const zcm::ReceiveBuffer* rbuf, 
                            const std::string& chan, 
                            const xbox_map_zcmt* msg);
    void IMU_state_ZCM(const zcm::ReceiveBuffer* rbuf, 
                            const std::string& chan, 
                            const state_estimator_zcmt* msg);
    void robot_info_ZCM(const zcm::ReceiveBuffer* rbuf, 
                         const std::string& chan,
                            const robot_info_zcmt* msg);    
    void error_log_ZCM(const zcm::ReceiveBuffer* rbuf,
                            const std::string& chan,
                            const error_log_zcmt* msg);

};

#endif